#include "trigger_response/action.hpp"
#include <cstdlib>

trigger_response_action::trigger_response_action(ros::NodeHandle & node,ros::NodeHandle & private_node) : node_(node), private_node_(private_node) {
    ugv_path_pub_ = node_.advertise<nav_msgs::Path>("car/response_path", 1);
    // target_pos_pub_ = node_.advertise<nav_msgs::Path>("car/target_pos", 1);
    target_pos_pub_ = node_.advertise<nav_msgs::Path>("car/response_path", 1);

    ugv_light_client_ = node_.serviceClient<agxcar_ctrl::lightcontrol>("lightcontrol");



    private_node_.param("waypoint_num", waypoint_num_, -1);
    for (int i = 0; i < waypoint_num_; i++)
    {
      private_node_.param("waypoint" + std::to_string(i) + "_x", waypoints_[i][0], -1.0);
      private_node_.param("waypoint" + std::to_string(i) + "_y", waypoints_[i][1], -1.0);
      private_node_.param("waypoint" + std::to_string(i) + "_z", waypoints_[i][2], -1.0);
    }
    std::cout << "waypoint_num_:" << waypoint_num_ << std::endl;

}


void trigger_response_action::trigger_response(const Eigen::Vector3d & UavPos, const Eigen::Vector3d & UgvPos, const  Eigen::Vector3d & TargetPos, int response_id){
    switch (response_id)
    {
    case response_type::HOLD_ON:
        ROS_INFO("Hold on, do nothing");
        break;
    
    case response_type::PATROL:
        ROS_INFO("Patrol");
        //TODO: 发送巡逻路径到 ugv_path_pub_
        // 注意给path赋值的时候，要把frame id 设置为world
        publish_patrol_path();
        close_light();

        break;


    case response_type::FOLLOW:
        ROS_INFO("follow");
        target_pos_pub(TargetPos);
        close_light();

        break;

    case response_type::ALARM:
        ROS_INFO("alarm");
        target_pos_pub(TargetPos);
        alarm_light();

        break;

    case response_type::ATTACK:
        ROS_INFO("attack");
        target_pos_pub(TargetPos);
        attack_light();

        break;

    

    default:
        ROS_ERROR("Unknown response id");
        break;
    }

}

void trigger_response_action::target_pos_pub(const Eigen::Vector3d & TargetPos){
    // 根据已知点生成路径
    nav_msgs::Path path_msg;
    path_msg.header.frame_id = "world";
    path_msg.header.stamp = ros::Time::now();

    //将巡逻路径添加进去
    geometry_msgs::PoseStamped poseStamped;
    poseStamped.header = path_msg.header; // 继承路径消息的头信息
    poseStamped.pose.position.x = TargetPos[0];
    poseStamped.pose.position.y = TargetPos[1];
    poseStamped.pose.position.z = TargetPos[2]; // 确保你的航点数据包含z坐标
    // 如果航点没有特定的朝向，可以设置为四元数的单位值，表示无旋转
    poseStamped.pose.orientation.w = 1.0;
    poseStamped.pose.orientation.x = 0.0;
    poseStamped.pose.orientation.y = 0.0;
    poseStamped.pose.orientation.z = 0.0;
    path_msg.poses.push_back(poseStamped);
    // 发布路径消息
    target_pos_pub_.publish(path_msg);


}



void trigger_response_action::publish_patrol_path(){
    // 根据已知点生成路径
    nav_msgs::Path path_msg;
    path_msg.header.frame_id = "world";
    path_msg.header.stamp = ros::Time::now();

    //将巡逻路径添加进去
    for (const auto& waypoint : waypoints_) {
        geometry_msgs::PoseStamped poseStamped;
        poseStamped.header = path_msg.header; // 继承路径消息的头信息
        std::cout << "waypoint1:" << waypoint[0] << std::endl;
        poseStamped.pose.position.x = waypoint[0];
        poseStamped.pose.position.y = waypoint[1];
        poseStamped.pose.position.z = waypoint[2]; // 确保你的航点数据包含z坐标
        // 如果航点没有特定的朝向，可以设置为四元数的单位值，表示无旋转
        poseStamped.pose.orientation.w = 1.0;
        poseStamped.pose.orientation.x = 0.0;
        poseStamped.pose.orientation.y = 0.0;
        poseStamped.pose.orientation.z = 0.0;
        path_msg.poses.push_back(poseStamped);
    }
    // 发布路径消息
    ugv_path_pub_.publish(path_msg);

}
void trigger_response_action::close_light(){
     // 创建服务客户端
    ros::ServiceClient client = node_.serviceClient<agxcar_ctrl::lightcontrol>("agxcar_ctrl/carcontrol/set_light_mode");
    // 定义服务请求
    agxcar_ctrl::lightcontrol srv;
    srv.request.lightIntensity = 0; // 设置灯光强度

    // 等待服务可用
    if (!client.waitForExistence(ros::Duration(5.0))) {
        ROS_ERROR("Service set_light_mode is not available.");
    }

    // 调用服务
    if (client.call(srv)) {
        ROS_INFO("Service called successfully: %s", srv.response.response ? "True" : "False");
    } else {
        ROS_ERROR("Failed to call service set_light_mode");
    }


}

void trigger_response_action::alarm_light(){
     // 创建服务客户端
    ros::ServiceClient client = node_.serviceClient<agxcar_ctrl::lightcontrol>("agxcar_ctrl/carcontrol/set_light_mode");
    // 定义服务请求
    agxcar_ctrl::lightcontrol srv;
    srv.request.lightIntensity = 1000.0; // 设置灯光强度

    // 等待服务可用
    if (!client.waitForExistence(ros::Duration(5.0))) {
        ROS_ERROR("Service set_light_mode is not available.");
    }

    // 调用服务
    if (client.call(srv)) {
        ROS_INFO("Service called successfully: %s", srv.response.response ? "True" : "False");
    } else {
        ROS_ERROR("Failed to call service set_light_mode");
    }
    // // 在这里执行Python脚本
    // // 使用system调用来执行Python脚本，并调用特定的函数
    // std::string command = "/usr/bin/python3 /home/laijun/kjw_proj/trigger_response_ws/src/trigger_response/src/lightApi.py perform_alarm_light";
    // int result = system(command.c_str());

    // if (result == 0) {
    //     std::cout << "Python script executed perform_alarm_light successfully." << std::endl;
    // } else {
    //     std::cout << "Python script execution perform_alarm_light failed." << std::endl;
    // }


}

void trigger_response_action::attack_light(){
    // 创建服务客户端
    ros::ServiceClient client = node_.serviceClient<agxcar_ctrl::lightcontrol>("agxcar_ctrl/carcontrol/set_light_mode");

    // 等待服务可用
    if (!client.waitForExistence(ros::Duration(5.0))) {
        ROS_ERROR("Service set_light_mode is not available.");
         // 如果服务不可用，则退出程序
    }

    // 定义服务请求
    agxcar_ctrl::lightcontrol srv;

    // 灯光闪烁5次
    const int num_flashes = 10;
    for (int i = 0; i < num_flashes; ++i) {
        // 打开灯光
        srv.request.lightIntensity = 1000.0; // 设置灯光强度为最大
        if (client.call(srv)) {
            ROS_INFO("Light turned on successfully: %s", srv.response.response ? "True" : "False");
        } else {
            ROS_ERROR("Failed to turn on light");
             // 如果服务调用失败，则退出程序
        }
        ros::Duration(1.0).sleep(); // 灯光开启持续1秒

        // 关闭灯光
        srv.request.lightIntensity = 0.0; // 设置灯光强度为0
        if (client.call(srv)) {
            ROS_INFO("Light turned off successfully: %s", srv.response.response ? "True" : "False");
        } else {
            ROS_ERROR("Failed to turn off light");
             // 如果服务调用失败，则退出程序
        }
        ros::Duration(1.0).sleep(); // 灯光关闭持续1秒
    }

    ROS_INFO("Light flashing completed.");

    // std::string command = "/usr/bin/python3 /home/laijun/kjw_proj/trigger_response_ws/src/trigger_response/src/lightApi.py perform_attack_light";
    // int result = system(command.c_str());

    // if (result == 0) {
    //     std::cout << "Python script executed perform_attack_light successfully." << std::endl;
    // } else {
    //     std::cout << "Python script execution perform_attack_light failed." << std::endl;
    // }
}